import lejos.nxt.Motor;
import lejos.robotics.subsumption.Behavior;

/**
 * 
 */
public class FaceGoal implements Behavior {
	private boolean suppressed = false;
	private int targetGoalInDegrees = 0;
	private int IROffsetMiddle = 90;
	private int IROffsetSides = 50;

	/**
	 * Face the goal.
	 */
	public void action() {
		suppressed = false;

		int previousSpeed_A = Motor.A.getSpeed();
		int previousSpeed_B = Motor.B.getSpeed();

		Motor.A.setSpeed(50);
		Motor.B.setSpeed(50);

		while (!isFacingCorrectDirection() && hasBall() && !suppressed) {
			if (!Motor.A.isMoving() && !Motor.B.isMoving()) {
				// check wHich side to rotate to
				if (BillManager.getCompassData() > 180) {
					Motor.A.backward();
					Motor.B.forward();
				} else {
					Motor.A.forward();
					Motor.B.backward();
				}
			}
		}

		Motor.A.stop();
		Motor.B.stop();

		Motor.A.setSpeed(previousSpeed_A);
		Motor.B.setSpeed(previousSpeed_B);

		if (isFacingCorrectDirection())
			BillManager.SetIsFacingGoal(true);

		// while ((Motor.A.isMoving() || Motor.B.isMoving()) && !suppressed)
		// Thread.yield();
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see lejos.robotics.subsumption.Behavior#suppress()
	 */
	public void suppress() {
		suppressed = true;
	}

	/**
	 * Only take control if we have the ball.
	 */
	public boolean takeControl() {
		// if (isFacingCorrectDirection())
		// BillManager.SetIsFacingGoal(true);

		return hasBall();
	}

	/**
	 * Calculates if the robot is facing the correct direction.
	 * 
	 * @return true if correct direction, false if not correct direction.
	 */
	private Boolean isFacingCorrectDirection() {
		// calculates the difference between the current compass degree value
		// and
		// the value that the robot needs to look at
		float difference = Math.abs(BillManager.getCompassData()
				- targetGoalInDegrees);
		if (difference > 180)
			difference = Math.abs(difference - 360);

		// The accepted difference in degree value
		float acceptedDifference = 20f;

		// robot is facing the target with a accepted difference
		if (difference < acceptedDifference)
			return true;

		return false;
	}

	/**
	 * Do we have the ball?
	 * 
	 * @return True if the ball is close, otherwise false.
	 */
	public boolean hasBall() {
		return ((BillManager.GetInfraredData() >= 4 && BillManager
				.GetInfraredData() <= 6) && (BillManager
				.GetInfraredSensor3Data() > IROffsetMiddle
				|| BillManager.GetInfraredSensor2Data() > IROffsetSides || BillManager
				.GetInfraredSensor4Data() > IROffsetSides));
	}
}
